%% 2024/03/03
% Surface Trajectory Tracking using Tube-based NMPC by Liang Hongpeng, 171186623@qq.com

%% Tips:
% 首先运行SDPT3-4.0下面的startup.m,否则提示找不到Solver

%% System description
% x' = f(x,u) + Bw * w

%% Initiallization
clear all % 清空所有变量
close all % 关闭所有图形窗口
clc % 清空命令行

%% 可调参数
T = 0.6;                                    %控制周期
N = 10;                                     % 预测时域 N * T

%% Disturbance
Bw = [1,0,0;0,1,0;0,0,3];                   % 扰动输入矩阵

wmax_x = 0.0006;                             % 状态x扰动的最大值0.03m/s 0.006 0.00006
wmax_y = 0.0006;                             % 状态y扰动的最大值0.03m/s 
wmax_theta = 0.0006;                         % 状态theta扰动的最大值0.09rad/s

wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);
W_vertex = [wmax_x , wmax_y , wmax_theta;   % 扰动集合的顶点
            wmax_x , -wmax_y, wmax_theta;
            -wmax_x, wmax_y , wmax_theta; 
            -wmax_x, -wmax_y, wmax_theta;
            wmax_x , wmax_y , -wmax_theta; 
            wmax_x , -wmax_y, -wmax_theta;
            -wmax_x, wmax_y , -wmax_theta; 
            -wmax_x, -wmax_y, -wmax_theta;];
% 在这个特定的代码中，W表示一个三维的矩形，表示在xyz轴上的扰动范围
W = Polyhedron(W_vertex);                   % 构造扰动集合的Polyhedron对象

load('./data/demo0.0006_3.mat'); % w

%% 路径规划
% 目标点3d pose A B
A=[0;4;0];
B = [0;0;30];                   %B=[0;0;10];
R = 2;                          %管道半径

[ref_x,ref_y,ref_vx,ref_vy] = PathPlan(A,B,R,T);
ns = length(ref_x);

ref_theta = atan2(ref_vy,ref_vx);
ref_v = sqrt(ref_vx.^2+ref_vy.^2);
%% 初始化

last_theta_r=1.57;
x_sim = [pi*R; 0; 1.57];            %理想2d pose
xc_sim = x_sim;                     %当前2d pose [pi*R; 0; pi/2]

x_sim_history = [];                 % 存储标称状态
x_sim_history(:,1) = x_sim;            
xc_sim_history = [];                % 存储实际状态
xc_sim_history(:,1) = x_sim;           
u_sim_history = [];                 % 存储标称输入
u_sim_history(:,1) = zeros(2,1);
uc_sim_history = [];                % 存储实际输入
uc_sim_history(:,1) = zeros(2,1);
ref_w = [];                         % 存储参考角速度输入

times = [];
times(1) = 0;

%w = zeros(3, ns);                  %存储扰动

%% 开始循环
for k=1:ns-(N-1)
%for k=1:54
    % 更新参考状态
    j = k+N-1;
    ref = [ref_x(k:j); ref_y(k:j); ref_theta(k:j)];
    v_r = ref_v(k);
    theta_r = ref_theta(k); w_r=angle_bound(theta_r-last_theta_r)/T; last_theta_r=theta_r;
    % u_r = [v_r;w_r]; % 致命的错误!(2024/04/07)
    u_r = [v_r;ref_theta(k)]; 
    % 从扰动集合中随机选择扰动
    %w(:,k) = pick_random_Disturbance(W);   

    % 在开始处记录时间
    iteration_start_time = tic;

    % 应用TubeNMPC方法控制律
    [uc_opt, u_opt] = TubeNMPC_Controller(ref, xc_sim, x_sim, u_r);

    % 输出耗时信息
    iteration_elapsed_time = toc(iteration_start_time);             % 计算循环耗时
    fprintf('Iteration %d took %.4f seconds\n', k, iteration_elapsed_time);

    % 将控制量输入对象
    x_sim = robot_model(x_sim, u_opt, T);                           % 理想系统
    xc_sim = robot_model(xc_sim,uc_opt,T) + 1*Bw * w(:,k);          % 真实带噪声

    % 保存历史数据
    u_sim_history = [u_sim_history, u_opt];
    x_sim_history = [x_sim_history, x_sim];
    uc_sim_history = [uc_sim_history, uc_opt];
    xc_sim_history = [xc_sim_history, xc_sim];                      % 实际轨迹记录
    ref_w = [ref_w, w_r];
    times = [times, k*T];

end

%% 绘图
% 图1：x,y,theta状态随时间变化情况
figure;
%x方向跟踪曲线
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(1, :))), ref_x(1:length(x_sim_history(1, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(1, :))), x_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(1, :))), xc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('x (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-x','nomi-x','tube-x'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

%y方向跟踪曲线
subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(2, :))), ref_y(1:length(x_sim_history(2, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(2, :))), x_sim_history(2, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(2, :))), xc_sim_history(2, :), 'g--', 'LineWidth', 0.8);
ylabel('y (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-y','nomi-y','tube-y'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

%转角跟踪曲线
subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(3, :))), ref_theta(1:length(x_sim_history(3, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(3, :))), x_sim_history(3, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(3, :))), xc_sim_history(3, :), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\theta','nomi-\theta','tube-\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图2：v,omega输入随时间变化情况
figure;
%线速度跟踪曲线
subplot(2, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(u_sim_history(1, :))), ref_v(1:length(u_sim_history(1, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(u_sim_history(1, :))), u_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(u_sim_history(1, :))), uc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('线速度 (m/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-v','nomi-v','tube-v'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

%角速度跟踪曲线
subplot(2, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(ref_w)), ref_w(1,:), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(ref_w)), u_sim_history(2, 1:length(ref_w)), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(ref_w)), uc_sim_history(2, 1:length(ref_w)), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('角速度 (rad/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\omega','nomi-\omega','tube-\omega'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图3：实际轨迹与参考轨迹之差 tube-ref
len = length(xc_sim_history);
tube_delta_x = ref_x(1:len) - xc_sim_history(1, :);
tube_delta_y = ref_y(1:len) - xc_sim_history(2, :);
tube_delta_theta = ref_theta(1:len) - xc_sim_history(3, :);

figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_x(1:len), 'r--', 'LineWidth', 0.8);
ylabel('\deltax (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltax'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_y(1:len), 'g--', 'LineWidth', 0.8);
ylabel('\deltay (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltay'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_theta(1:len), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\delta\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\delta\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图4：噪声
len=len-1;
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(1,1:len), 'r--', 'LineWidth', 0.8);
ylabel('wx (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wx'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(2,1:len), 'g--', 'LineWidth', 0.8);
ylabel('wy (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wy'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(3,1:len), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('w\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'w\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

%% 实际3d轨迹绘制
len=len+1;
figure();
PathPaint(ref_x(1:len),ref_y(1:len),x_sim_history(1,:),x_sim_history(2,:),xc_sim_history(1,:),xc_sim_history(2,:),R);